#include "rads_api.hpp"

rads::rads_acks acks_down;
ros::Publisher down;
// 用于向guard发布网络层信息
ros::Publisher ver;

// 全局原子变量，用于线程间的通信
std::atomic<bool> should_stop(false);
std::condition_variable cv;
std::mutex mtx;

// 获取设备配置信息
void device_callback(int device_id)
{
                std::string result = "";
                result = ApiDeviceGet(device_id);
        if (!result.empty()) {
                json jsonObj = nlohmann::json::parse(result);
                if (jsonObj["code"] == 200 && jsonObj["msg"] == "success") {
                    // MAXSPEED: 30                                  #自驾运行速度 #####
                    // RTK_F: 1                                      #1:使用千寻 2：自组网  ######
                    // QX_USER: "qxwfue0015@@@"                      #rtk账号  #######
                    // QX_USERPASS: "bb4c867@@@"                     #rtk密码  #######
                    // RADAR_F: 1                                    #1:启用避障 2:关闭避障 

                    //                     vehicleCode	"1500016"
                    // runSpeed	30
                    // useMode	1
                    // rtkAccount	"qxwfue0015"
                    // rtkPwd	"bb4c867"
                    // useObs	1
                    int Device_MAXSPEED= jsonObj["data"]["runSpeed"];
                    std::string command_Device_MAXSPEED = "cd /home/argo/catkin_ws/src/rads-master/shell && ./device.sh MAXSPEED " + std::to_string(Device_MAXSPEED);
                    std::cout << command_Device_MAXSPEED << std::endl;
                    system(command_Device_MAXSPEED.c_str());
                    ros::param::set("MAXSPEED", Device_MAXSPEED);

                    int Device_RTK_F=jsonObj["data"]["useMode"];
                    std::string command_Device_RTK_F = "cd /home/argo/catkin_ws/src/rads-master/shell && ./device.sh RTK_F " + std::to_string(Device_RTK_F);
                    system(command_Device_RTK_F.c_str());
                    ros::param::set("RTK_F", Device_RTK_F);

                    int Device_RADAR_F=jsonObj["data"]["useObs"];
                    std::string command_Device_RADAR_F = "cd /home/argo/catkin_ws/src/rads-master/shell && ./device.sh RADAR_F " + std::to_string(Device_RADAR_F);
                    system(command_Device_RADAR_F.c_str());
                    ros::param::set("RADAR_F", Device_RADAR_F);

                    std::string Device_QX_USER=jsonObj["data"]["rtkAccount"];
                    std::string command_Device_QX_USER = "cd /home/argo/catkin_ws/src/rads-master/shell && ./device.sh QX_USER '\"" + Device_QX_USER + "\"'";
                    system(command_Device_QX_USER.c_str());
                    ros::param::set("QX_USER", Device_QX_USER);

                    std::string Device_QX_USERPASS=jsonObj["data"]["rtkPwd"];;
                    std::string command_Device_QX_USERPASS = "cd /home/argo/catkin_ws/src/rads-master/shell && ./device.sh QX_USERPASS '\"" + Device_QX_USERPASS + "\"'";
                    system(command_Device_QX_USERPASS.c_str());
                    ros::param::set("QX_USERPASS", Device_QX_USERPASS);


                    std::cout << "\n============ 设备参数 ============\n"
                            << "| 自驾运行速度(MAXSPEED): " << Device_MAXSPEED << "\n"
                            << "| RTK模式(RTK_F): " << Device_RTK_F << " (1:千寻 2:自组网)\n"
                            << "| RTK账号(QX_USER): " << Device_QX_USER << "\n"
                            << "| RTK密码(QX_USERPASS): " << Device_QX_USERPASS << "\n"  // 密码用*号显示
                            << "| 避障状态(RADAR_F): " << Device_RADAR_F << " (1:启用 0:关闭)\n"
                            << "=================================\n" << std::endl;                    
                    // publish_date = jsonObj["data"]["publish_date"];
                    // version.publish_type = jsonObj["data"]["publish_type"];
                    // _netver.versions.push_back(version);
                    // ver.publish(_netver);
                }
        }

}


// 版本查询接口
void version_callback(const rads::rads_versions& _ver)
{
    rads::rads_versions _netver;
    std::string result = "";
    switch(_ver.ack)
    {
        // 获取网络版本号
        case  0:
            {
                result = ApiVersionGet();
                json jsonObj = nlohmann::json::parse(result);
                if (jsonObj["code"] == 200 && jsonObj["msg"] == "success") {
                    _netver.header.stamp = ros::Time::now();
                    _netver.header.frame_id = "nyd_net_version";
                    _netver.ack = 1;
                    _netver.versions.clear();
                    rads::rads_version version;
                    version.publish_date = jsonObj["data"]["publish_date"];
                    version.publish_type = jsonObj["data"]["publish_type"];
                    version.package_size = jsonObj["data"]["package_size"];
                    version.is_must = jsonObj["data"]["is_must"];
                    version.node_name = jsonObj["data"]["node_name"];
                    version.process_name = jsonObj["data"]["process_name"];
                    version.major_minor_patch = jsonObj["data"]["major_minor_patch"];
                    version.md5_code = jsonObj["data"]["md5_code"];
                    version.temp_down_path = jsonObj["data"]["temp_down_path"];
                    version.description = jsonObj["data"]["description"];
                    version.ext = jsonObj["data"]["ext"];
                    version.ext2 = jsonObj["data"]["ext2"];
                    _netver.versions.push_back(version);
                    ver.publish(_netver);
                }
            }
            break;
        default:
            break;
    }
}

// 用于对下载地图或线路的确认[暂时预留]
void ack_callback(const rads::rads_acks& _ack)
{
    switch (_ack.cmd)
    {
            case 4:
                {
                    if(CMDO_ACK_BEGIN == _ack.ack && 1 == _ack.cmd_opt)
                    {
                        std::string result = "";
                        API_PcdUpdate_img upd;
                        upd.id = std::to_string(_ack.ext2);
                        upd.file = _ack.map_path;
                        result = ApiPcdUpload_img(upd);
                        if(std::string::npos!=result.find("success"))
                        {
                            ROS_WARN("Description Synchronizing img [%zu] data succeeded!",_ack.ext2);
                            char command[256];
                            snprintf(command, sizeof(command),
                                    "/home/argo/catkin_ws/src/rads-master/shell/delete_img.sh %d &", 
                                    (int)_ack.ext2);

                            int ret = system(command);
                            if (ret != 0) {
                                ROS_WARN("Failed to execute delete command for image %d", (int)_ack.ext2);
                            }
                        }else{
                            ROS_ERROR("Description Synchronizing img [%zu] data failed!",_ack.ext2);
                        }
                    }
                }
                break;
        case CMDO_MAP_BUILD:
            // 开始建图确认
            if(CMDO_ACK_BEGIN == _ack.ack && CMDO_BEGIN == _ack.ext2)
            {
                std::string result = ApiPcdCreateReady(_ack.map_id,std::string(KTV_SN));
                json jsonObj = nlohmann::json::parse(result);
                if (jsonObj["code"] == 200 && jsonObj["msg"] == "success") {
                    ROS_WARN("CMDO_MAP_BUILD CONFIRM SIGNAL SUCCESS");
                }else{
                    ROS_ERROR("CMDO_MAP_BUILD CONFIRM SIGNAL FAIL.");
                }
            }
            break;
        case CMDO_PATH_BUILD:
            // 开始规划线路确认
            if(CMDO_ACK_BEGIN == _ack.ack)
            {
                 ROS_INFO("CMDO_PATH_BUILD CONFIRM SIGNAL, map_id[%ld]",_ack.map_id);
                 // to call api
            }
        case CMDO_MAP_DOWN:
            // 地图下载确认
            if(CMDO_ACK_LOADING == _ack.ack)
            {
                ROS_INFO("CMDO_MAP_DOWN CONFIRM FROM JING, map_id[%ld]",_ack.map_id);
            }
            break;
        case CMDO_PATH_DOWN:
            // 轨迹下载确认
            if(CMDO_ACK_LOADING == _ack.ack)
            {
                ROS_INFO("CMDO_ACK_LOADING CONFIRM FROM JING , path[%ld]",_ack.path_id);
            }
            break;
        case CMDO_SET_INIT_POSI:
            // 确认上传
             if(CMDO_ACK_BEGIN == _ack.ack)
             {
                std::string convertJson,result;
                json jsonData;
                if(_ack.trajectory.poses.size() > 0 && _ack.trajectory_ext.size() > 0)
                {
                    jsonData["vMapId"] = _ack.map_id;
                    jsonData["pose_x"] = _ack.trajectory.poses[0].pose.position.x;
                    jsonData["pose_y"] = _ack.trajectory.poses[0].pose.position.y;
                    jsonData["pose_z"] = _ack.trajectory.poses[0].pose.position.z;
                    jsonData["pose_orix"] = _ack.trajectory.poses[0].pose.orientation.x;
                    jsonData["pose_oriy"] = _ack.trajectory.poses[0].pose.orientation.y;
                    jsonData["pose_oriz"] = _ack.trajectory.poses[0].pose.orientation.z;
                    jsonData["pose_oriw"] = _ack.trajectory.poses[0].pose.orientation.w;
                    jsonData["xvalue"] = _ack.trajectory_ext[0].x;
                    jsonData["yvalue"] = _ack.trajectory_ext[0].y;
                    jsonData["zvalue"] = _ack.trajectory_ext[0].z;
                    jsonData["yaw"] = _ack.trajectory_ext[0].yaw;
                    jsonData["lng"] = _ack.trajectory_ext[0].longitude;
                    jsonData["lat"] = _ack.trajectory_ext[0].latitude;
                    jsonData["alt"] = _ack.trajectory_ext[0].altitude;
                    jsonData["heading"] = _ack.trajectory_ext[0].heading;
                    jsonData["posiFlag"] = _ack.trajectory_ext[0].posi_flag;
                    jsonData["headFlag"] = _ack.trajectory_ext[0].head_flag;
                    jsonData["score"] = _ack.trajectory_ext[0].matching_rate;
                    jsonData["station"] = "";
                }
                convertJson = jsonData.dump(4);
                result = ApiInitPoseUpload(convertJson);
                json jsonObj = nlohmann::json::parse(result);
                if (!(jsonObj["code"] == 200 && jsonObj["msg"] == "success")) {
                    ROS_ERROR("CMDO_SET_INIT_POSI UPLOAD FAILED.");
                }else{
                    ROS_WARN("ApiInitPoseUpload Success");
                }
             }
            break;
        // 任意点导航增预览轨迹
        case CMDO_PASSPOINT_NAV:
            if(CMDO_ACK_LOADING == _ack.ack)
            {
                std::string convertJson,result;
                int pointscount = 0;
                json jsonData;
                json vRoutes = json::array();
                json vRoute;
                json routePoints = json::array();
                jsonData["vMapId"] = _ack.map_id;
                jsonData["vMapId"] = 128;
                jsonData["vCmd"] = _ack.pro_nema;
                vRoute["vRouteId"] = -1;
                for (rads::rads_trackext track : _ack.trajectory_ext) {
                    pointscount++;
                    json routePoint;
                    routePoint["vMapId"] = _ack.map_id;
                    routePoint["vRouteId"] = -1;
                    routePoint["station"] = "";
                    routePoint["sort"] = track.point_count;
                    routePoint["xvalue"] = track.x;
                    routePoint["yvalue"] = track.y;
                    routePoint["zvalue"] = track.z;
                    routePoint["yaw"] = track.yaw;
                    routePoint["lng"] = track.longitude;
                    routePoint["lat"] = track.latitude;
                    routePoint["alt"] = track.altitude;
                    routePoint["heading"] = track.heading;
                    routePoint["posiFlag"] = track.posi_flag;
                    routePoint["headFlag"] = track.head_flag;
                    routePoint["score"] = track.matching_rate;
                    routePoints.push_back(routePoint);
                }
                vRoute["countPoints"] = pointscount;
                vRoute["routePoints"] = routePoints;
                vRoutes.push_back(vRoute);
                jsonData["vRoutes"] = vRoutes;
                convertJson = jsonData.dump(4);
  //            ROS_WARN("CMDO_PASSPOINT_NAV convertJson = \n%s",convertJson.c_str());
                result = ApiTrajectoryUpload(convertJson);
                json jsonObj = nlohmann::json::parse(result);
                if (!(jsonObj["code"] == 200 && jsonObj["msg"] == "success")) {
                    ROS_ERROR("CMDO_PASSPOINT_NAV TRAJECTORY UPLOAD FAILED");
                }else{
                    ROS_WARN("ApiTrajectoryUpload Success");
                }
            }
            break;
        default:
            {
                // 用于任务调度的实时更新
                if(CMDO_ACK_PROGRESS == _ack.ack)
                {
                    std::string result = ApiPcdRefreshProgress(std::string(KTV_SN),_ack.progress);
                    json jsonObj = nlohmann::json::parse(result);
                    if (jsonObj["code"] == 200 && jsonObj["msg"] == "success") {
                        ROS_WARN("The task progress is updated successful");
                    }else{
                        ROS_ERROR("The task progress is updated failed.");
                    }
                }
            }
            break;
    }
}

// 用于地图和线路下载
void cmd_callback(const rads::rads_cmd& _cmd)
{
    switch (_cmd.pro_cmd)
    {
        case CMDO_MAP_DOWN:
            {
                // 确认解析API服务接口JSON数据
                ROS_WARN("Start calling downloading pcd api .......");
                std::string ret = ApiPcdDownload(_cmd.pro_cmd_opt);
                json jsonObj = nlohmann::json::parse(ret);
                if (jsonObj["code"] == 200 && jsonObj["msg"] == "success") {
                    std::string path = jsonObj["data"]["url"];
                    int vMapId = jsonObj["data"]["vMapId"];
                    if(_cmd.pro_cmd_opt == vMapId)
                    {
                        ROS_WARN("Start downloading %s",path.c_str());
                        auto start = std::chrono::high_resolution_clock::now();
                        ApiPcdDown(path,std::to_string(vMapId));
                        auto end = std::chrono::high_resolution_clock::now();
                        std::chrono::duration<double, std::milli> duration = end - start;
                        acks_down.vehicle_id = std::string(KTV_SN);
                        acks_down.cmd = _cmd.pro_cmd;
                        acks_down.cmd_opt = _cmd.pro_cmd_opt;
                        acks_down.ext1 = _cmd.pro_ext1;
                        acks_down.ext2 = _cmd.pro_ext2;
                        acks_down.map_id = vMapId;
                        acks_down.map_path = std::string(MAP_PCD_SAVE) + std::to_string(vMapId) + ".pcd";
                        acks_down.ack = 1;
                        acks_down.header.stamp = ros::Time::now();
                        acks_down.header.frame_id = "nyd-map";
                        json& poses = jsonObj["data"]["initPose"];
                        for (json& pose : poses) {
                            geometry_msgs::PoseStamped _pose;
                            rads::rads_trackext _track;
                            _pose.pose.position.x = pose["pose_x"];
                            _pose.pose.position.y = pose["pose_y"];
                            _pose.pose.position.z = pose["pose_z"];
                            _pose.pose.orientation.x = pose["pose_orix"];
                            _pose.pose.orientation.y = pose["pose_oriy"];
                            _pose.pose.orientation.z = pose["pose_oriz"];
                            _pose.pose.orientation.w = pose["pose_oriw"];
                            _track.x = pose["xvalue"];
                            _track.y = pose["yvalue"];
                            _track.z = pose["zvalue"];
                            _track.yaw = pose["yaw"];
                            _track.longitude = pose["lng"];
                            _track.latitude = pose["lat"];
                            _track.altitude = pose["alt"];
                            _track.heading = pose["heading"];
                            _track.posi_flag = pose["posiFlag"];
                            _track.head_flag = pose["headFlag"];
                            _track.matching_rate = pose["score"];
                            acks_down.trajectory.poses.push_back(_pose);
                            acks_down.trajectory_ext.push_back(_track);
                        }
                        down.publish(acks_down);
                        ROS_WARN("Taking %f milliseconds,save in %s",duration.count(),acks_down.map_path.c_str());
                    }else{
                        ROS_ERROR("Map numbers do not match Request[%ld] response[%d]",_cmd.pro_cmd_opt,vMapId);
                    }
                } else {
                    ROS_ERROR("CMDO_MAP_DOWN FAILED");
                }
            }
            break;
        case CMDO_PATH_DOWN:
            {
                std::string ret = ApipathDown(_cmd.pro_cmd_opt,_cmd.pro_ext1);
                json jsonObj = nlohmann::json::parse(ret);
                if (jsonObj["code"] == 200 && jsonObj["msg"] == "success") {
                    int vMapId = jsonObj["data"]["vMapId"];
                    acks_down.vehicle_id = std::string(KTV_SN);
                    acks_down.cmd = _cmd.pro_cmd;
                    acks_down.cmd_opt = _cmd.pro_cmd_opt;
                    acks_down.ext1 = _cmd.pro_ext1;
                    acks_down.ext2 = _cmd.pro_ext2;
                    acks_down.map_id = vMapId;
                    json& vRoutes = jsonObj["data"]["vRoutes"];
                    for (json& route : vRoutes) {
                        acks_down.path_id = route["vRouteId"];
                        int countPoints = route["countPoints"];
                        json& routePoints = route["routePoints"];
                        rads::rads_trackext tra;
                        acks_down.trajectory_ext.clear();
                        for (json& point : routePoints) {
                            int point_count = countPoints;
                            float x = point["xvalue"];
                            float y = point["yvalue"];
                            float z = point["zvalue"];
                            float yaw = point["yaw"];
                            int posi_flag = point["posiFlag"];
                            int head_flag = point["headFlag"];
                            float longitude = point["lng"];
                            float latitude = point["lat"];
                            float altitude = point["alt"];
                            float heading = point["heading"];
                            float matching_rate = point["score"];
                            int ext = point["sort"];
                            tra.point_count = ext;
                            tra.x = x;
                            tra.y = y;
                            tra.z = z;
                            tra.yaw = yaw;
                            tra.posi_flag = posi_flag;
                            tra.head_flag = head_flag;
                            tra.longitude = longitude;
                            tra.latitude = latitude;
                            tra.altitude = altitude;
                            tra.heading = heading;
                            tra.matching_rate = matching_rate;
                            acks_down.trajectory_ext.push_back(tra);
                        }
                        acks_down.ack = 1;
                        acks_down.header.stamp = ros::Time::now();
                        acks_down.header.frame_id = "nyd-path";
                        down.publish(acks_down);
                        ROS_WARN("begin to send points [%d],path_id = %zu",countPoints,acks_down.path_id);
                        std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    }
                }else {
                    ROS_ERROR("CMDO_PATH_DOWN FAILED");
                }
            }
            break;
         case 4:  //配置文件主动更新40(速度，千寻)
            {
                if(_cmd.pro_cmd_opt==0){
                    ros::param::get("KTV_SN",KTV_SN);
                    int device_id = std::stoi(KTV_SN);
                    device_callback(device_id);

                }
                
            }
            break;
        default:
            break;
    }
}

// 涉及建图,定位点上传
void api_callback(const rads::rads_apis& _api)
{
    try{
        std::string result = "";
        switch (_api.cmd)
        {
            // 地图上传
            case CMDO_MAP_BUILD:
                {
                    if(CMDO_END == _api.ext)
                    {
                        API_PcdUpdate upd;
                        upd.vid = std::string(KTV_SN);
                        upd.code = upd.vid;
                        upd.mapid = std::to_string(_api.map_id);
                        upd.file = _api.map_path;
                        #ifdef DEV_LOCAL
                        	upd.file = std::string(MAP_PCD_TEST);
                        #endif
                        result = ApiPcdUploadNotice(upd);
                        result = ApiPcdUpload(upd);
                        if(std::string::npos!=result.find("success"))
                        {
                            ROS_WARN("Description Synchronizing map [%zu] data succeeded!",_api.map_id);
                        }else{
                            ROS_ERROR("Description Synchronizing map [%zu] data failed!",_api.map_id);
                        }
                    }
                }
                break;
            // 地图上传
            case CMDO_PATH_BUILD:
                {
                    if(CMDO_ACK_LOADING == _api.ext)
                    {
                        API_PointUpdate upd;
                        upd.map_id = _api.map_id;
                        upd.path_id = _api.path_id;
                        upd.point_count = _api.trajectory_ext.point_count;
                        upd.x = _api.trajectory_ext.x;
                        upd.y = _api.trajectory_ext.y;
                        upd.z = _api.trajectory_ext.z;
                        upd.yaw = _api.trajectory_ext.yaw;
                        upd.matching_rate = _api.trajectory_ext.matching_rate;
                        upd.lng = _api.trajectory_ext.longitude;
                        upd.lat = _api.trajectory_ext.latitude;
                        upd.alt = _api.trajectory_ext.altitude;
                        upd.heading = _api.trajectory_ext.heading;
                        upd.posi_flag = _api.trajectory_ext.posi_flag;
                        upd.head_flag = _api.trajectory_ext.head_flag;
                        result = ApiPointUpload(upd);
                        if(std::string::npos!=result.find("success"))
                        {
                            ROS_WARN("Point [%04d->%f,%f,%f,%f] location is synchronized succeeded",upd.point_count,upd.x,upd.y,upd.lng,upd.lat);
                        }else{
                            ROS_ERROR("Point [%04d->%f,%f,%f,%f] location is synchronized failed",upd.point_count,upd.x,upd.y,upd.lng,upd.lat);
                        }
                    }
                }
                break;
            default:
                break;
        }
    }catch (const std::exception& e) {
        ROS_ERROR("Exception caught in api_callback: %s", e.what());
    }
}


// 双光谱报警
void camerainfo_callback(const rads::rads_camera& _api)
{
    API_Camera upd;
    
    // 先从消息中复制所有字段
    upd.device_id = _api.device_id;
    upd.devIp = _api.devIp;
    upd.Alarmtype = _api.Alarmtype;
    upd.preset = _api.preset;
    upd.preAlarmSum = _api.preAlarmSum;
    upd.alarmSum = _api.alarmSum;
    upd.mtAlarmInfo = _api.mtAlarmInfo;
    upd.file = _api.file;
    upd.thermalfile = _api.thermalfile;
    
    // 然后检查文件路径是否有效
    if(upd.file != "")
    {
        std::string result = ApiCameraUpload(upd);
        if(std::string::npos != result.find("success"))
        {
            ROS_WARN("camerainfo location is synchronized succeeded");
        }
        else
        {
            ROS_ERROR("camerainfo location is synchronized failed");
        }
    }
}

//任务包cronapi
void cronapi_callback(const rads::rads_cronapi& _api)
{
    API_Cron upd;
    
    // 先从消息中复制所有字段
    upd.ts = _api.ts;
    upd.uid = _api.uid;
    upd.vehicleCode = _api.vehicleCode;
    upd.executeType = _api.executeType;
    upd.taskCode = _api.taskCode;
    upd.num = _api.num;
    upd.status = _api.status;  //  1 开始，2结束，3失败
    upd.mapid =_api.mapid;
    upd.turn =_api.turn;
    upd.uptime = _api.uptime;
    // 然后检查文件路径是否有效
    if (upd.taskCode != "0") 
    {
        std::string result = ApiCronUpload(upd);
        if(std::string::npos != result.find("success"))
        {
             ROS_INFO("Cronapi sync succeeded [task:%zu, num:%zu, status:%zu]",
                    upd.taskCode, upd.num, upd.status);
        }
        else
        {
           ROS_ERROR("Cronapi sync failed [task:%zu, num:%zu, status:%zu, error:%s]",
                    upd.taskCode, upd.num, upd.status, result.c_str());
        }
    }
}


// 信号处理函数
void signalHandler(int signum) {
    if(SIGINT == signum){
        system("killall -s SIGTERM rads_guard");
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        ROS_INFO("[Api] Interrupt signal (%d) received. Shutting down...", signum);
        std::lock_guard<std::mutex> lck(mtx);
        should_stop.store(true);
        cv.notify_all();
        ros::shutdown();
    }
}

int main(int argc, char **argv)
{
    ROS_INFO("[Api] Starting....");
    ros::init(argc, argv, NODE_API);
    ros::NodeHandle nh;
    ros::param::get("KTV_SN",KTV_SN);
    int device_id = std::stoi(KTV_SN);
    device_callback(device_id);
    // 设置信号处理函数
    signal(SIGINT, signalHandler);
    ros::Subscriber s = nh.subscribe("/pradsapi", QUEUE_SIZE,api_callback);
    ros::Subscriber s2 = nh.subscribe("/pradscmd", QUEUE_SIZE,cmd_callback);
    ros::Subscriber s3 = nh.subscribe("/sradsack", QUEUE_SIZE,ack_callback);
    ros::Subscriber s4 = nh.subscribe("/radsversion2api", QUEUE_SIZE,version_callback);
    ros::Subscriber s5 = nh.subscribe("/pradscamerainfo", QUEUE_SIZE,camerainfo_callback);
    ros::Subscriber s6 = nh.subscribe("/radsscronapi", QUEUE_SIZE,cronapi_callback);

    ver = nh.advertise<rads::rads_versions>("/radsversionapi", QUEUE_SIZE);
    down = nh.advertise<rads::rads_acks>("/sradsack", QUEUE_SIZE);
    ros::spin();
    ROS_INFO("[Api] closed....");
    return 0;
}